#include "fluid.h"

#include <GL\glew.h>

#define GLFW_NO_GLU
#include <gl\glfw.h>

#include <cmath>
#include <omp.h>



#include <basecode\misc_utils.h>


//#define parallel_for_loopz

float roygbiv[256][3];


	FluidSolver::FluidSolver()
	{

		N = 1024;
		G             = (real) 0.006;				// Gravitational Constant for our simulation
		spacing       = 3.0;						// Spacing of particles
		k			  = (real) (spacing / 1000.0);	// Far pressure weight
		k_near		  = k*10;						// Near pressure weight
		rest_density  = 10;							// Rest Density
		sigma         = 4;							// visco 1
		beta          = 3;							// visco 2
		r = (real) (spacing*6.25);					// Radius of Support
		rsq = r*r;									// ... squared for performance stuff
		SIM_W         = 600;						// The size of the world
		bottom        = 0;							// The floor of the world

		attracting = 0;
		repelling  = 0;
		attractor.x = -SIM_W;
		attractor.y = -SIM_W;


		initParticles();
		generateColorLUT();

		glGenBuffers(1, &VertexVBOID);
		glBindBuffer(GL_ARRAY_BUFFER, VertexVBOID);
		glBufferData(GL_ARRAY_BUFFER, verts.size()*sizeof(glm::vec2), NULL, GL_DYNAMIC_DRAW); // GL_STREAM_DRAW - per render update, GL_DYNAMIC_DRAW per frame
	}


	void FluidSolver::render()
	{
		real xs = 1;
		real ys = SIM_W+1;

		static float _minVelocity = 0;
		static float _maxVelocity = 0;

		float mindens = -rest_density;
		
		// To disable it all
		//glDisable(GL_POINT_SPRITE);
		//glDisable(GL_BLEND);
		//glDisable(GL_TEXTURE_2D);
		//glPointSize(1);

		glPointSize(r*2);
		glEnable( GL_BLEND);
		glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_CONSTANT_COLOR); // used with glBlendColorEXT, asserts default state is glBlendColor(0,0,0,1);
		glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
		glDepthMask(GL_FALSE);


		if( particlesCreated > 0 )
		{
			double before_upload = glfwGetTime();
#ifdef USE_VBO
			verts.clear();

			for(int i=0; i<particlesCreated; i++){
				glm::vec2 p = glm::vec2( particles[i].pos.x, SIM_W - particles[i].pos.y );
				verts.push_back( p );
			}

			
			//glEnableClientState(GL_VERTEX_ARRAY);
			//glVertexPointer( 2, GL_FLOAT, 0, &verts[0] );
			//glDrawArrays(GL_POINTS, 0, verts.size() );
			//glDisableClientState(GL_VERTEX_ARRAY);

			glBindBuffer( GL_ARRAY_BUFFER, VertexVBOID );
			glEnableClientState(GL_VERTEX_ARRAY);
			#define BUFFER_OFFSET(i) ((char *)NULL + (i))
			glVertexPointer(2, GL_FLOAT, sizeof(glm::vec2), BUFFER_OFFSET(0));   //The starting point of the VBO, for the vertices

			glBufferData(GL_ARRAY_BUFFER, verts.size()*sizeof(glm::vec2), &verts[0].x, GL_DYNAMIC_DRAW); // Use insead of sub, cause num particles changes...
			//glBufferSubData(GL_ARRAY_BUFFER, 0, verts.size()*sizeof(glm::vec2), &verts[0].x);
			glDrawArrays(GL_POINTS, 0, verts.size() );
			glDisableClientState(GL_VERTEX_ARRAY);
#else
					glBegin(GL_POINTS);
					for(int i=0; i < particlesCreated; ++i)
					{
			//#define velocitycolor
			#ifdef velocitycolor
						float velMagnitude = glm::length( particles[i].vel );
						if( velMagnitude < _minVelocity )
						{
							_minVelocity = velMagnitude;
						}
						if( velMagnitude > _maxVelocity )
						{
							_maxVelocity = velMagnitude * 0.86f;
						}
						float finalC = ( (_minVelocity+velMagnitude)/_maxVelocity  );
						int c = std::min( std::max( (int) (170.0f - finalC*170.0f), 0 ), 170 );
						glColor3fv( roygbiv[c] ); // same as glColor3f( roygbiv[c][0], roygbiv[c][1], roygbiv[c][2] );	
			#else  // Use pressure for color
						float opacity = ( particles[i].rho );
						//float f = 2.0f * particles[i].force.len();
						//opacity-=mindens;
						opacity/=(rest_density-mindens);
						float finalC = opacity*3;
						//glColor4f( f, f, opacity, opacity);
						int c = std::min( std::max( (int) (170.0f - finalC*170.0f), 0 ), 170 );
						glColor3fv( roygbiv[c] ); // same as glColor3f( roygbiv[c][0], roygbiv[c][1], roygbiv[c][2] );		
			#endif
						glVertex2f( (float) (xs + particles[i].pos.x) , (float) (ys + -particles[i].pos.y) );
					}
					// Done drawing points
					glEnd();
#endif

			uploadTime = glfwGetTime() - before_upload;
		}
	}


	void FluidSolver::update()
	{
		// UPDATE
		//
		// This modified verlet integrator has dt = 1 and calculates the velocity
		// For later use in the simulation. 

		// For each particles i ...	

#ifdef parallel_for_loopz
		#pragma omp parallel for
#endif
		for(int i=0; i < particlesCreated; ++i)
		{
			// Apply the currently accumulated forces
			particles[i].pos += particles[i].force;

			// Restart the forces with gravity only. We'll add the rest later.
			particles[i].force = glm::vec2(0, -G);

			// Calculate the velocity for later.
			particles[i].vel = particles[i].pos - particles[i].pos_old;

			// (damping has been removed, since there can never be instability issues
			//  in a verlet based physics simulation, and since it was an ugly hack :-)

			// Normal verlet stuff
			particles[i].pos_old = particles[i].pos;
			particles[i].pos += particles[i].vel;

			// If the particle is outside the bounds of the world, then
			// Make a little spring force to push it back in.
			if(particles[i].pos.x < 0) particles[i].force.x -= (particles[i].pos.x ) / 2;
			if(particles[i].pos.x >  SIM_W) particles[i].force.x -= (particles[i].pos.x - SIM_W) / 2;
			if(particles[i].pos.y < bottom) particles[i].force.y -= (particles[i].pos.y - bottom) / 2;
			if(particles[i].pos.y > SIM_W)particles[i].force.y -= (particles[i].pos.y - SIM_W) / 2;

			// Handle mouse interaction. 

			glm::vec2 p_to_attr =  (particles[i].pos - attractor);
			real attr_dist2 = glm::dot(p_to_attr, p_to_attr); // Dot with self is the same as dist squared
			real attr_l = 32;

			//  make mouse attract particles
			if( attracting )
				if( attr_dist2 < attr_l*attr_l && attr_dist2 > 16 ){
					glm::normalize(p_to_attr);
					particles[i].force -= p_to_attr * 0.1f;// * (1.0f / 64.0f );
				}


				//  make mouse repel particles
				if( repelling )
					if( attr_dist2 < attr_l*attr_l ){
						particles[i].force += (particles[i].pos - attractor) * (1.0f / 64.0f);
					}

					// Reset the nessecary items.
					particles[i].rho = 0;
					particles[i].rho_near = 0;
					particles[i].neighbors.clear(); // Clear neighbors
		}

		// DENSITY 
		//
		// Calculate the density by basically making a weighted sum
		// of the distances of neighboring particles within the radius of support (r)

		// For each particle ...
#ifdef parallel_for_loopz
#pragma omp parallel for
#endif
		for(int i=0; i < particlesCreated-1; ++i)
		{

			// We will sum up the 'near' and 'far' densities.
			real d=0;
			real dn=0;

			// Now look at every other particle
			for(int j = i+1; j < particlesCreated; ++j)
			{

				//  OPTIMISATION!! skip pair if either x or y distance
				//  is bigger than the support radius, r. This seems to 
				//  boost speed a notch.
				if(fabs(particles[j].pos.x - particles[i].pos.x) > r) continue;
				if(fabs(particles[j].pos.y - particles[i].pos.y) > r) continue;

				// The vector seperating the two particles
				glm::vec2 rij = particles[j].pos - particles[i].pos;

				// Along with the squared distance between
				real rij_magnitudesquared = glm::dot( rij, rij );         

				// If they're within the radius of support ...
				if(rij_magnitudesquared < rsq)
				{
					// Get the actual distance from the squared distance.
					real rij_len = sqrtf(rij_magnitudesquared);

					// And calculated the weighted distance values
					real q = (real) 1.0 - rij_len / r;
					real q2 = q*q;
					real q3 = q2*q;

					d  += q2;
					dn += q3;

					// Accumulate on the neighbor
					particles[j].rho += q2;
					particles[j].rho_near += q3;

					// Set up the neighbor list for faster access later.          
					neighbor n;
					n.i = i;
					n.j = j;
					n.q = q;
					n.q2 = q2;
					n.lng = rij_len;
					particles[i].neighbors.push_back( n );
				}
			}
			particles[i].rho      += d;
			particles[i].rho_near += dn;
		}

		// PRESSURE
		//
		// Make the simple pressure calculation from the equation of state.
#ifdef parallel_for_loopz
#pragma omp parallel for
#endif
		for(int i=0; i < particlesCreated; ++i)
		{
			particles[i].press = k * (particles[i].rho - rest_density);
			particles[i].press_near = k_near * particles[i].rho_near;
		}

		// PRESSURE FORCE
		//
		// We will force particles in or out from their neighbors
		// based on their difference from the rest density.

		// For each particle ...
		int ni;
		#ifdef parallel_for_loopz
		#pragma omp parallel for private(ni)
		#endif
		for(int i=0; i < particlesCreated; ++i)
		{
			glm::vec2 dX(0.0f, 0.0f);

			// For each of the neighbors
			int numNeighs = particles[i].neighbors.size();
			for(ni=0; ni < numNeighs; ++ni)
			{
				unsigned int j = particles[i].neighbors[ni].j;     
				real q = particles[i].neighbors[ni].q;
				real q2 = particles[i].neighbors[ni].q2;       

				// The vector from particle i to particle j
				glm::vec2 rij = particles[j].pos - particles[i].pos;

				// calculate the force from the pressures calculated above
				real dm = (particles[i].press + particles[j].press) * q +
					(particles[i].press_near + particles[j].press_near) * q2;

				// Get the direction of the force
				//vec2f D = rij.normal() * dm;
				real forceMag = (real) 1.0/particles[i].neighbors[ni].lng * dm;
				glm::vec2 D = rij * forceMag;    //  OPTIMISATION!! here we save a sqrt() call.
				dX += D;
				particles[j].force += D;
			}
			particles[i].force -= dX;
		}

}

void FluidSolver::activateAttractor( int mx, int my )
{
	attracting = 1;
	attractor = glm::vec2( (real )mx, -my + SIM_W);	
}

void FluidSolver::activateRepelling( int mx, int my )
{
	repelling = 1;
	attractor = glm::vec2( (real )mx, -my + SIM_W);	
}

void FluidSolver::deactivateAttractor()
{
	attracting = 0;
	repelling = 0;
}

void FluidSolver::addParticles( int mx, int my )
{
	attractor.x = (real) mx;
	attractor.y = (real) -my + SIM_W;	
	if (particlesCreated < MAX_PARTICLES)
	{
		real x = attractor.x + MISRENDER::frand() * 14 ;
		real y = attractor.y + MISRENDER::frand() * 14;
		particles[particlesCreated].pos = glm::vec2(x,y);
		particles[particlesCreated].pos_old = glm::vec2(x,y);  
		particles[particlesCreated].force = glm::vec2(0,0);
		particles[particlesCreated].sigma = sigma;
		particles[particlesCreated].beta = beta;

		particlesCreated++;
	}
}

void FluidSolver::initParticles()
{
	// create a world with dimensions x:[-SIM_W,SIM_W] and y:[0,SIM_W*2]
	// Initialize particles
	// We will make a block of particles with a total width of 1/2 of the screen.
	particlesCreated = 0;

	real x = 0;
	real y = SIM_W/2;
	for(int i=0; i<N; i++)
	{
		x += r * (real)0.25;
		if( x > SIM_W/4)
		{ 
			x = 0;
			y += (real) r * (real)0.5;
		}

		if(particlesCreated > N) break;

		particles[particlesCreated].pos = glm::vec2(x,y);
		particles[particlesCreated].pos_old = glm::vec2(x,y);
		particles[particlesCreated].force = glm::vec2(0,0);
		particles[particlesCreated].sigma = sigma;
		particles[particlesCreated].beta = beta;

		particlesCreated++;			
	}
	verts.resize( N );
}

void FluidSolver::generateColorLUT()
{
	// Generate color look-up table
	double hue = 0;
	double hueInc = 6.0/255.0;
	double v = 1.0; // value 
	double s = 1.0; // saturation
	for(int i=0; i<175; i++)
	{
		double c = v * s;
		double x = c * 1.0-fabs(fmod(hue,2.0)-1.0);
		//x*=255;
		//c*=255;
#define RGB(i, r,g,b) roygbiv[i][0] = (float)r; roygbiv[i][1] = (float)g; roygbiv[i][2] = (float)b;
		if(hue >= 0 && hue <= 1){ RGB(i, c,x,0); }// red->orange
		if(hue >= 1 && hue <= 2){ RGB(i, x,c,0); }// orange->yellow
		if(hue >= 2 && hue <= 3){ RGB(i, 0,c,x); }// yellow->green
		if(hue >= 3 && hue <= 4){ RGB(i, 0,x,c); }// green->blue
		if(hue >= 4 && hue <= 5){ RGB(i, x,0,c); }// blue->indigo
		if(hue >= 5 && hue <= 6){ RGB(i, c,0,x); }// indigo->violet
#undef RGB
		hue += hueInc;
	}
}

FluidSolver::~FluidSolver()
{

	glDeleteBuffers(1, &VertexVBOID );
}
